/******************************************************************************
 * Copyright (C) 2023 Advanced Micro Devices, Inc. All Rights Reserved.
 * SPDX-License-Identifier: MIT
 ******************************************************************************/
/*
 * helloworld.c: simple test application
 *
 * This application configures UART 16550 to baud rate 9600.
 * PS7 UART (Zynq) is not initialized by this application, since
 * bootrom/bsp configures it to baud rate 115200
 *
 * ------------------------------------------------
 * | UART TYPE   BAUD RATE                        |
 * ------------------------------------------------
 *   uartns550   9600
 *   uartlite    Configurable only in HW design
 *   ps7_uart    115200 (configured by bootrom/bsp)
 */

#include "PWM.h"
#include "platform.h"
#include "xgpio.h"
#include "xil_printf.h"
#include "xparameters.h"
#include "xtime_l.h"
#include "xtimer_config.h"
#include <math.h> //需要在CMakelists.txt中的target_link_libraries(...)最后加上m，变成target_link_libraries(... m)，才能正常使用数学库
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>

#define FRAME_RATE 100
#define PRINT_FRAME_RATE 30

#define LED_NUM 4
#define BRIGHTEN 1
#define DARKEN -1

u32 Period = 100000;
int Breath_Rate_Level[LED_NUM] = {10, 10, 10, 10};

int LED_OFFSET[LED_NUM] = {
    PWM_PWM_AXI_SLV_REG0_OFFSET, PWM_PWM_AXI_SLV_REG1_OFFSET,
    PWM_PWM_AXI_SLV_REG2_OFFSET, PWM_PWM_AXI_SLV_REG3_OFFSET};

int pwm_ctrl_focus = 0;

XTime start_time, current_time, start_print_time;

int init_XGpio(XGpio *InstancePtr, UINTPTR BaseAddress);
void set_PWM_Values(u32 PWM_Values[LED_NUM], u32 ButtonInput);
void write_PWM_Values_reg(u32 PWM_Values[LED_NUM], u32 SwitchInput);
void show_Status(u32 PWM_Values[LED_NUM], u32 SwitchInput, u32 ButtonInput);

int main() {
  xil_printf("\033[2J"); // 发送 ANSI 转义序列清空屏幕
  xil_printf("\033[H"); // 发送 ANSI 转义序列将光标移动到屏幕左上角
  init_platform();
  print("Hello World\n\r");
  print("Successfully ran Hello World application\n\r");

  // init XGPIO device
  XGpio Switch, Button;
  if (init_XGpio(&Switch, XPAR_DIP_SWITCH_GPIO_BASEADDR) != XST_SUCCESS) {
    return XST_FAILURE;
  }
  if (init_XGpio(&Button, XPAR_PUSH_BUTTON_GPIO_BASEADDR) != XST_SUCCESS) {
    return XST_FAILURE;
  }

  // get the Input of Switch and Button
  u32 SwitchInput = 0, ButtonInput = 0;

  // store pwm_values
  u32 PWM_Values[LED_NUM] = {9999, 9999, 9999, 9999};
  // set Period
  PWM_mWriteReg(XPAR_PWM_0_BASEADDR, PWM_PWM_AXI_SLV_REG4_OFFSET, Period);

  XTime_GetTime(&start_time);
  XTime_GetTime(&start_print_time);
  while (1) {
    SwitchInput = XGpio_DiscreteRead(&Switch, 1);
    ButtonInput = XGpio_DiscreteRead(&Button, 1);

    set_PWM_Values(PWM_Values, ButtonInput);
    write_PWM_Values_reg(PWM_Values, SwitchInput);
    show_Status(PWM_Values, SwitchInput, ButtonInput);
  }

  cleanup_platform();
  return 0;
}

int init_XGpio(XGpio *InstancePtr, UINTPTR BaseAddress) {
  xil_printf("init XGPIO device with BaseAddress = %x\n\r", BaseAddress);
  XGpio_Config *Config = XGpio_LookupConfig(BaseAddress);
  return XGpio_CfgInitialize(InstancePtr, Config, Config->BaseAddress);
}

int get_bit(u32 num, size_t index) {
  // get the index bit of num
  return (num & (1 << index)) >> index;
}

void show_4bit(u32 num) {
  for (size_t i = 0; i < 4; i++) {
    xil_printf("%d", get_bit(num, i));
  }
  print("\n\r");
  return;
}

void show_PWM_Values(u32 PWM_Values[LED_NUM], u32 SwitchInput) {
  for (size_t i = 0; i < LED_NUM; i++) {
    xil_printf("LED%d: %6d/%6d at breathing rate level %2d\r\n", i,
               PWM_Values[i] * get_bit(SwitchInput, i), Period,
               Breath_Rate_Level[i]);
  }
  return;
}

void show_Input(u32 SwitchInput, u32 ButtonInput) {
  xil_printf("SwitchInput: %2d<==> ", SwitchInput);
  show_4bit(SwitchInput);
  xil_printf("ButtonInput: %2d<==> ", ButtonInput);
  show_4bit(ButtonInput);
  return;
}

void set_PWM_Values(u32 PWM_Values[LED_NUM], u32 ButtonInput) {
  XTime_GetTime(&current_time);
  float tUsed = (current_time - start_time + 0.0) / COUNTS_PER_SECOND;
  if (tUsed < 1.0 / FRAME_RATE) {
    return;
  }
  start_time = current_time;

  static int PWM_Status[LED_NUM] = {BRIGHTEN, BRIGHTEN, BRIGHTEN, BRIGHTEN};
  static u32 previousButtonInput = -1;

  // Use ButtonInput[3:2] to switch pwm_ctrl_focus
  if (previousButtonInput != ButtonInput) {
    pwm_ctrl_focus =
        pwm_ctrl_focus + get_bit(ButtonInput, 3) - get_bit(ButtonInput, 2);
  }
  if (pwm_ctrl_focus > LED_NUM - 1) {
    pwm_ctrl_focus = 0;
  } else if (pwm_ctrl_focus < 0) {
    pwm_ctrl_focus = LED_NUM - 1;
  } else {
    // normal status
  }

  // Use ButtonInput[1:0] to change the Breate_Rate_Level
  if (previousButtonInput != ButtonInput) {
    if (get_bit(ButtonInput, 1)) {
      Breath_Rate_Level[pwm_ctrl_focus]++;
    }
    if (get_bit(ButtonInput, 0)) {
      Breath_Rate_Level[pwm_ctrl_focus]--;
    }
  }

  previousButtonInput = ButtonInput;

  for (size_t i = 0; i < LED_NUM; i++) {
    switch (PWM_Status[i]) {
    case BRIGHTEN:
      PWM_Values[i] =
          PWM_Values[i] * (Breath_Rate_Level[i] / 100.0 + 1) + rand() % 8;
      break;
    case DARKEN:
      PWM_Values[i] = PWM_Values[i] / (Breath_Rate_Level[i] / 100.0 + 1);
      break;
    default:
      print("PWM_Status error\r\n");
      break;
    }

    if (PWM_Values[i] < 1) {
      PWM_Values[i] = 1;
      PWM_Status[i] = BRIGHTEN;
    } else if (PWM_Values[i] > Period) {
      PWM_Values[i] = Period;
      PWM_Status[i] = DARKEN;
    } else {
      // normal range
    }
  }
  return;
}

void write_PWM_Values_reg(u32 PWM_Values[LED_NUM], u32 SwitchInput) {
  for (size_t i = 0; i < LED_NUM; i++) {
    PWM_mWriteReg(XPAR_PWM_0_BASEADDR, LED_OFFSET[i],
                  PWM_Values[i] * get_bit(SwitchInput, i));
  }
  return;
}

void show_Status(u32 PWM_Values[LED_NUM], u32 SwitchInput, u32 ButtonInput) {
  XTime_GetTime(&current_time);
  float tUsed = (current_time - start_print_time + 0.0) / COUNTS_PER_SECOND;
  if (tUsed < 1.0 / PRINT_FRAME_RATE) {
    return;
  }
  start_print_time = current_time;

  static int first_show = 1;
  if (first_show) {
    first_show = 0;
  } else {
    xil_printf("\033[8F");
  }

  print("push BTN3/2 to switch focus\r\n");
  show_Input(SwitchInput, ButtonInput);
  xil_printf("push BTN1/0 to Increase/Decrease the breathing rate of LED%d\n\r",
             pwm_ctrl_focus);
  show_PWM_Values(PWM_Values, SwitchInput);
  return;
}